/*
 * RobotDrive2012.cpp
 *
 *  Created on: Feb 11, 2012
 *      Author: trwulff
 */

#include "RobotDrive2012.hpp"
#ifndef ROBOTDRIVE2012_CPP_
#define ROBOTDRIVE2012_CPP_
/*
 * Default constructor
 */
	/*
	 * Default constructor
	 */
	RobotDrive2012::RobotDrive2012(UINT8 LeftMotorChannel,UINT8 RightMotorChannel,float P,float I,float D,UINT16 EncoderLPR)
					:Jag1(LeftMotorChannel,CANJaguar::kSpeed),
					Jag2(RightMotorChannel,CANJaguar::kSpeed)
	//				,Drive2012(Jag1,Jag2)
	{
		
		Jag1.EnableControl(0);
		Jag2.EnableControl(0);
		Jag1.SetPID(P, I, D);		// TODO tune PID numbers
		Jag2.SetPID(P, I, D);
		Jag1.ConfigEncoderCodesPerRev(ENCODER_LPR); // 250 lpr in quadrature mode (4x) 
		Jag2.ConfigEncoderCodesPerRev(ENCODER_LPR);
		Jag1.SetExpiration(30);
		Jag2.SetExpiration(30);
		Jag1.SetSafetyEnabled(false);
		Jag2.SetSafetyEnabled(false);
		iCount=200;

	}
	/*
	 * Default destructor
	 */
	RobotDrive2012::~RobotDrive2012()
	{
		
	}
	void RobotDrive2012::SetPID(double P,double I,double D)
	{
		Jag1.SetPID(P, I, D);
		Jag2.SetPID(P, I, D);
	}
	void RobotDrive2012::SetEncoderLPR(UINT16 LPR)
	{
		Jag1.ConfigEncoderCodesPerRev(ENCODER_LPR);  
		Jag2.ConfigEncoderCodesPerRev(ENCODER_LPR);
	}
	/**
	 * Arcade drive implements single stick driving.
	 * This function lets you directly provide joystick values from any source.
	 * @param moveValue The value to use for fowards/backwards
	 * @param rotateValue The value to use for the rotate right/left
	 * @param squaredInputs If set, increases the sensitivity at low speeds
	 */
	
	void RobotDrive2012::ArcadeDrive(float moveValue , float  rotateValue)
	{
		// local variables to hold the computed PWM values for the motors
		float leftMotorOutput;
		float rightMotorOutput;

		moveValue = Limit(moveValue);
		rotateValue = Limit(rotateValue);

		
			// square the inputs (while preserving the sign) to increase fine control while permitting full power
			if (moveValue >= 0.0)
			{
				moveValue = (moveValue * moveValue);
			}
			else
			{
				moveValue = -(moveValue * moveValue);
			}
			if (rotateValue >= 0.0)
			{
				rotateValue = (rotateValue * rotateValue);
			}
			else
			{
				rotateValue = -(rotateValue * rotateValue);
			}
		

		if (moveValue > 0.0)
		{
			if (rotateValue > 0.0)
			{
				leftMotorOutput = moveValue - rotateValue;
				rightMotorOutput = max(moveValue, rotateValue);
			}
			else
			{
				leftMotorOutput = max(moveValue, -rotateValue);
				rightMotorOutput = moveValue + rotateValue;
			}
		}
		else
		{
			if (rotateValue > 0.0)
			{
				leftMotorOutput = - max(-moveValue, rotateValue);
				rightMotorOutput = moveValue + rotateValue;
			}
			else
			{
				leftMotorOutput = moveValue - rotateValue;
				rightMotorOutput = - Max(-moveValue, -rotateValue);
			}
		}
		if(iCount <=0)
		{
//		printf("Lft=%f, Rt= %f\n",leftMotorOutput,rightMotorOutput);
		iCount = 200;
		}
		iCount--;
		Jag1.Set(leftMotorOutput);
		Jag2.Set(rightMotorOutput);
			
	}

	void RobotDrive2012::SetPosition(float Position)
	{
		Jag1.Set(Position);
		Jag2.Set(Position);
	}

	
	
	
	
	
	
	float RobotDrive2012::Max(float V1,float V2)
	{
		if(V1>V2)return V1;
		return V2;
	}
	
	float RobotDrive2012::Limit(float Value)
	{
	 if(Value<-1)return -1;
	 if(Value>1)return 1;
	 return Value;
	 
	}
	void RobotDrive2012::SetNeutral(CANJaguar::NeutralMode mode)
	{
		Jag1.ConfigNeutralMode(mode);
		Jag2.ConfigNeutralMode(mode);
	}
	void RobotDrive2012::SetSafetyEnabled (bool safetyEnabled)
	{
		Jag1.SetSafetyEnabled(safetyEnabled);
		Jag2.SetSafetyEnabled(safetyEnabled);
	}
	
	void RobotDrive2012::SetExpiration (float timeout)
	{
		Jag1.SetExpiration(timeout);
		Jag2.SetExpiration(timeout);
	}

	void RobotDrive2012::Printf()
	{
		if(Jag1.GetControlMode()==CANJaguar::kSpeed)printf("Mode = speed");
		if(Jag1.GetControlMode()==CANJaguar::kVoltage)printf("Mode = voltage");
		if(Jag1.GetControlMode()==CANJaguar::kPosition)printf("Mode = position");
		printf("   PID={%f, %f, %f}",Jag1.GetP(),Jag1.GetI(),Jag1.GetD());
		printf("   left speed = %f",Jag1.GetSpeed());
		printf("   right speed = %f",Jag2.GetSpeed());
		printf("\n");
	}

#endif // end
